#include "sys.h"
#include "g_code.h"
#include "interpolator.h"
// #define USE_PC_DEBUG

#ifdef USE_PC_DEBUG
#include <stdio.h>
#endif

g_code_sys_status_t g_code_coords_mode = G_CODE_COORDS_MODE_RELATIVE;

g_code_sys_status_t g_code_current_coords = COORDINATE_1;

g_code_sys_status_t g_code_work_plane = PLANE_X_Y;

g_code_sys_status_t g_code_interpolator = G_CODE_INTERPOLATOR_QUICK;

float g_code_feed_rate = 0;

float (*g_code_interpolate_handler[4])(g_code_param_t*param); 

int g_code_unsupported(g_code_param_t *param)
{
#ifdef USE_PC_DEBUG
    printf("G%2d:Unsupported instrument\n",param->g);
#endif
    return -1;
}

#define X_Y_Z_Calculate() \
CNC_Coordinate_t coord; \
if(g_code_coords_mode != G_CODE_COORDS_MODE_RELATIVE){ \
    coord.x = ((param->flags) & G_CODE_FLAGS_X_BIT)?(param->x):0; \
    coord.y = ((param->flags) & G_CODE_FLAGS_Y_BIT)?(param->y):0; \
    coord.z = ((param->flags) & G_CODE_FLAGS_Z_BIT)?(param->z):0; \
    coord.x += System_RefCoordinate.x; \
    coord.y += System_RefCoordinate.y; \
    coord.z += System_RefCoordinate.z; \
} \
else { \
    coord.x = ((param->flags) & G_CODE_FLAGS_X_BIT)?(param->x):System_RefCoordinate.x; \
    coord.y = ((param->flags) & G_CODE_FLAGS_Y_BIT)?(param->y):System_RefCoordinate.y; \
    coord.z = ((param->flags) & G_CODE_FLAGS_Z_BIT)?(param->z):System_RefCoordinate.z; \
} \

static int G0_Handler(g_code_param_t *param)
{
    //obsoleted
    // X_Y_Z_Calculate();
    // CNC_GoToCoordinate(&coord);
    g_code_interpolator = G_CODE_INTERPOLATOR_QUICK;
    return 0;
}

static int G1_Handler(g_code_param_t *param)
{
    g_code_interpolator = G_CODE_INTERPOLATOR_LINE;
    return 0;
}
static int G2_Handler(g_code_param_t *param)
{
    g_code_interpolator = G_CODE_INTERPOLATOR_CW;
    return 0;
}
static int G3_Handler(g_code_param_t *param)
{
    g_code_interpolator = G_CODE_INTERPOLATOR_CCW;
    return 0;
}

static int G90_Handler(g_code_param_t *param)
{
    g_code_coords_mode = G_CODE_COORDS_MODE_ABSOLUTE;
    return 0;
}

static int G91_Handler(g_code_param_t *param)
{
    g_code_coords_mode = G_CODE_COORDS_MODE_RELATIVE;
    return 0;
}

static int G4_Handler(g_code_param_t *param)
{
#ifdef USE_PC_DEBUG
    printf("FastMove to (%f,%f,%f)\n", param->x, param->y, param->z);
#endif
    return 0;
}

// static int G15___G29_Handler(g_code_param_t *param)
// {
// #ifdef USE_PC_DEBUG
//     printf("FastMove to (%f,%f,%f)\n", param->x, param->y, param->z);
// #endif
//     return 0;
// }


// static int G90_G91_Handler(g_code_param_t *param)
// {
//     if(param->g == 90)
//     {
//         g_code_coords_mode = G_CODE_COORDS_MODE_ABSOLUTE;
//     }
//     else
//     {
//         g_code_coords_mode = G_CODE_COORDS_MODE_RELATIVE;
//     }
// #ifdef USE_PC_DEBUG
//     printf("G%02d,current %s\n",param->g, (g_code_coords_mode == G_CODE_COORDS_MODE_ABSOLUTE)?"Absolute":"Releative");
// #endif
//     return 0;
// }

static int M3_Handler(g_code_param_t *param)
{
    CNC_SpindleControl(1);
}
static int M4_Handler(g_code_param_t *param)
{
    CNC_SpindleControl(2);
}
static int M5_Handler(g_code_param_t *param)
{
    CNC_SpindleControl(0);
}

static int M8_Handler(g_code_param_t *param)
{
    CNC_CoolingControl(1);
}

static int M9_Handler(g_code_param_t *param)
{
    CNC_CoolingControl(0);
}

int g_code_g_handler_hashfunc(int G_NUMBER)
{
    int ret;
    if(G_NUMBER<5)
    {
        ret = G_NUMBER + 1;
    }
    else if(G_NUMBER > 14 && G_NUMBER < 30)
    {
        ret = 6;
    }
    else if(G_NUMBER == 90 || G_NUMBER == 91)
    {
        ret = 7;
    }
    else
    {
        ret = 0;
    }
    return ret;
}
int g_code_m_handler_hashfunc(int M_NUMBER)
{
    int ret;
    ret = 0;
}
g_code_handler_t g_handler[120] = {g_code_unsupported};
g_code_handler_t m_handler[36] = {g_code_unsupported};

#define G_CODE_HANDLER_G_REGESTER(i) (g_handler[i]=G##i##_Handler)
#define G_CODE_HANDLER_M_REGESTER(i) (m_handler[i]=M##i##_Handler)

int g_code_handler_init(void)
{
    G_CODE_HANDLER_G_REGESTER(0);
    G_CODE_HANDLER_G_REGESTER(1);
    G_CODE_HANDLER_G_REGESTER(2);
    G_CODE_HANDLER_G_REGESTER(3);
    G_CODE_HANDLER_G_REGESTER(90);
    G_CODE_HANDLER_G_REGESTER(91);

    G_CODE_HANDLER_M_REGESTER(3);
    G_CODE_HANDLER_M_REGESTER(4);
    G_CODE_HANDLER_M_REGESTER(5);
    G_CODE_HANDLER_M_REGESTER(8);
    G_CODE_HANDLER_M_REGESTER(9);
}